/*
 * Copyright (C) 2020 1km Group Holding Limited
 */

#include "dm556_driver.h"
#include "door.h"

#define DOOR_OPEN_DIR 1
#define DOOR_CLOSE_DIR 0
#define DORR_STATE_CHECK_TIME 10

#define DOOR_LEFT_OPEN_LIMIT1 4
#define DOOR_LEFT_OPEN_LIMIT2 12
#define DOOR_LEFT_CLOSE_LIMIT1 5
#define DOOR_LEFT_CLOSE_LIMIT2 13

#define DOOR_RIGHT_OPEN_LIMIT1 14
#define DOOR_RIGHT_OPEN_LIMIT2 16
#define DOOR_RIGHT_CLOSE_LIMIT1 15
#define DOOR_RIGHT_CLOSE_LIMIT2 17

#define DOOR_LEFT_BRAKE 9
#define DOOR_RIGHT_BRAKE 10
#define MAGNETIC_DOOR 11

//int32_t door1_speed = 13000;
//int32_t door2_speed = 9000;
int32_t door1_speed = 26000;
int32_t door2_speed = 18000;

int32_t door_slow_down_speed = 1000000;
bool door_state = 0;
bool door_first_operate = 1;

int16_t door_err = 0;


can_node_id_t door_node_id_list[4] = {{PORT_CAN_RESERVED, 2}, {PORT_CAN_RESERVED, 3},//推门操作
									{PORT_CAN_RESERVED, 4}, {PORT_CAN_RESERVED, 5}};//合门操作



//后面看一下门的错误如何处理
void xp_door_state_check()
{
	while(1){
		//TODO:check door limit state,ERR process;
		aos_msleep(50);
	}
}

/**
 * @brief 控制关门通用操作，控制两个驱动器关门并检测IO信号进行停止，
 *		  开门超时表示门故障受阻，记录错误码
 * 
 * @param node_id1 驱动器1CAN端口与从站节点号 
 * @param node_id2 驱动器2CAN端口与从站节点号
 * @param limit_num1 检测IO1 
 * @param limit_num2 检测IO2
 * @param time_out 关门超时时间
 * @return int32_t 0成功，非0表示失败
 */
static int32_t xp_door_close_common(const can_node_id_t *node_id1, const can_node_id_t *node_id2,
										   int16_t limit_num1, int16_t limit_num2, int16_t time_out, int32_t door_speed)
{
	int32_t ret = 0;
	int16_t polling_count = 0;
	bool door_close_complete1 = 0; 	
	bool door_close_complete2 = 0; 

	if(0 == time_out){
		time_out = 40;//默认大约2s
	}

	polling_count = time_out/DORR_STATE_CHECK_TIME;

	if(xp_io_read_input_pin(1, limit_num1)!=0){
		ret = set_spin_direction(node_id1, DOOR_OPEN_DIR);
		if(0 != ret){
			return -1;
		}
		ret = set_target_velocity(node_id1, door_speed);
		if(0 != ret){
			return -1;
		}
	}

	if(xp_io_read_input_pin(1, limit_num2)!=0){
		ret = set_spin_direction(node_id2, DOOR_CLOSE_DIR);
		if(0 != ret){
			return -1;
		}
		ret = set_target_velocity(node_id2, door_speed);
		if(0 != ret){
			return -1;
		}
	}

	ret = clear_slow_down(node_id1);
	if(0 != ret){
		return -1;
	}
	ret = clear_slow_down(node_id2);
	if(0 != ret){
		return -1;
	}

	while(polling_count--){
		//如果检测到限位IO输入则触发停止电机
		if(0 == xp_io_read_input_pin(1, limit_num1) &&  0 == door_close_complete1){
			ret = slow_down(node_id1, door_slow_down_speed);
			if(0 != ret){
				return -1;
			}
			door_close_complete1 = 1;
		}

		if(0 == xp_io_read_input_pin(1, limit_num2) && 0 == door_close_complete2){
			ret = slow_down(node_id2, door_slow_down_speed);
			if(0 != ret){
				return -1;
			}
			door_close_complete2 = 1;
		}

		if(door_close_complete1 && door_close_complete2){
			return 0;
		}
		
		aos_msleep(DORR_STATE_CHECK_TIME);
	}

	door_err = 1;
	return -1;

}

/**
 * @brief 控制门的关闭
 * 
 * @param node_id CAN端口与从站节点号 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t xp_door_close()
{
	int32_t ret = 0;

	if(0 == door_first_operate){
		if(0 == door_state){
			printf("door already close \n");
			return -2;
		}
	}
	xp_io_output_set(1, DOOR_LEFT_BRAKE, 0);
	xp_io_output_set(1, DOOR_RIGHT_BRAKE, 0);
	xp_io_output_set(1, MAGNETIC_DOOR, 0);
	aos_msleep(100);
	//这里limit IO输入号后面需要确认
	ret = xp_door_close_common(&door_node_id_list[0], &door_node_id_list[1], DOOR_LEFT_CLOSE_LIMIT2 , 
															DOOR_RIGHT_CLOSE_LIMIT2, 10000, door2_speed);
	if(0 != ret){
		return -1;
	}

	//这里limit IO输入号后面需要确认
	ret = xp_door_close_common(&door_node_id_list[2], &door_node_id_list[3], DOOR_LEFT_CLOSE_LIMIT1 , 
															DOOR_RIGHT_CLOSE_LIMIT1, 10000, door1_speed);
	if(0 != ret){
		return -1;
	}
	
	xp_io_output_set(1, DOOR_LEFT_BRAKE, 1);
	xp_io_output_set(1, DOOR_RIGHT_BRAKE, 1);

	door_first_operate = 0;
	door_state = 0;
	
	return 0;
}

/**
 * @brief 控制开门通用操作，控制两个驱动器开门并检测IO信号进行停止，
 *		  开门超时表示门故障受阻，记录错误码
 * 
 * @param node_id1 驱动器1CAN端口与从站节点号 
 * @param node_id2 驱动器2CAN端口与从站节点号
 * @param limit_num1 检测IO1 
 * @param limit_num2 检测IO2
 * @param time_out 开门超时时间
 * @return int32_t 0成功，非0表示失败
 */
static int32_t xp_door_open_common(const can_node_id_t *node_id1, const can_node_id_t *node_id2,
										   int16_t limit_num1, int16_t limit_num2, int16_t time_out, int32_t door_speed)
{
	int32_t ret = 0;
	int16_t polling_count = 0;
	bool door_open_complete1 = 0; 	
	bool door_open_complete2 = 0; 

	if(0 == time_out){
		time_out = 40;//默认大约2s
	}

	polling_count = time_out/DORR_STATE_CHECK_TIME;

	if(xp_io_read_input_pin(1, limit_num1)!=0){
		ret = set_spin_direction(node_id1, DOOR_CLOSE_DIR);
		if(0 != ret){
			return -1;
		}

		ret = set_target_velocity(node_id1, door_speed);
		if(0 != ret){
			return -1;
		}
	}

	if(xp_io_read_input_pin(1, limit_num2)!=0){
		ret = set_spin_direction(node_id2, DOOR_OPEN_DIR);
		if(0 != ret){
			return -1;
		}

		ret = set_target_velocity(node_id2, door_speed);
		if(0 != ret){
			return -1;
		}
	}

	ret = clear_slow_down(node_id1);
	if(0 != ret){
		return -1;
	}
	ret = clear_slow_down(node_id2);
	if(0 != ret){
		return -1;
	}

	while(polling_count--){
		//如果检测到限位IO输入则触发停止电机
		if(0 == xp_io_read_input_pin(1, limit_num1) &&  0 == door_open_complete1){
			ret = slow_down(node_id1, door_slow_down_speed);
			if(0 != ret){
				return -1;
			}
			door_open_complete1 = 1;
		}

		if(0 == xp_io_read_input_pin(1, limit_num2) && 0 == door_open_complete2){
			ret = slow_down(node_id2, door_slow_down_speed);
			if(0 != ret){
				return -1;
			}
			door_open_complete2 = 1;
		}

		if(door_open_complete1 && door_open_complete2){
			return 0;
		}
		
		aos_msleep(DORR_STATE_CHECK_TIME);
	}

	door_err = 1;
	return -1;

}

/**
 * @brief 控制门的打开
 * 
 * @return int32_t 0成功，非0表示失败
 */
int32_t xp_door_open()
{
	int32_t ret = 0;

	if(1 == door_state){
		printf("door already open \n");
		return -2;
	}
	
	//打开抱闸以及门磁吸
	xp_io_output_set(1, DOOR_LEFT_BRAKE, 0);
	xp_io_output_set(1, DOOR_RIGHT_BRAKE, 0);
	xp_io_output_set(1, MAGNETIC_DOOR, 1);
	aos_msleep(100);
	ret = xp_door_open_common(&door_node_id_list[2], &door_node_id_list[3], DOOR_LEFT_OPEN_LIMIT1 , 
														DOOR_RIGHT_OPEN_LIMIT1, 10000, door1_speed);
	if(0 != ret){
		return -1;
	}

	ret = xp_door_open_common(&door_node_id_list[0], &door_node_id_list[1], DOOR_LEFT_OPEN_LIMIT2 ,
														DOOR_RIGHT_OPEN_LIMIT2, 10000, door2_speed);
	if(0 != ret){
		return -1;
	}

	door_state = 1;
	xp_io_output_set(1, DOOR_LEFT_BRAKE, 1);
	xp_io_output_set(1, DOOR_RIGHT_BRAKE, 1);

	return 0;
}


/**
 * @brief 门的操作初始化
 * 
 * @return int32_t 0成功，非0表示失败
 */
int32_t xp_door_init()
{
	int32_t ret = 0;

	if(-1 == xp_canopen_dev_manager_init()){
		return -1;
	}

	//等待伺服进入canopen操作状态
	aos_msleep(5000);
	
	ret = enable_dm556(&door_node_id_list[0]);
	if(0 != ret){
		return -1;
	}
	ret = enable_dm556(&door_node_id_list[1]);
	if(0 != ret){
		return -1;
	}

	ret = enable_dm556(&door_node_id_list[2]);
	if(0 != ret){
		return -1;
	}
	ret = enable_dm556(&door_node_id_list[3]);
	if(0 != ret){
		return -1;
	}

	ret = profile_velocity_config(&door_node_id_list[0], 3000, 10000, 10000);
	if(0 != ret){
		return -1;
	}
	ret = profile_velocity_config(&door_node_id_list[1], 3000, 10000, 10000);
	if(0 != ret){
		return -1;
	}
	ret = profile_velocity_config(&door_node_id_list[2], 3000, 10000, 10000);
	if(0 != ret){
		return -1;
	}
	ret = profile_velocity_config(&door_node_id_list[3], 3000, 10000, 10000);
	if(0 != ret){
		return -1;
	}

	void xp_door_pressTest_thread(void *arg);
	aos_task_new("door test",xp_door_pressTest_thread,NULL,1024);
	return 0;
}


static aos_sem_t press_sem;
static int press_stop=1;
/*
door press test
*/
void xp_door_pressTest_thread(void *arg){
	aos_sem_new(&press_sem,0);
	press_stop=1;
	while(1){
		aos_sem_wait(&press_sem,AOS_WAIT_FOREVER);
		xp_door_open();
		aos_msleep(1000);
		xp_door_close();
		if(press_stop==1){
			press_stop=0;
			continue;
		}
		aos_msleep(1000);
		aos_sem_signal(&press_sem);
	}
}

/**********************DEBUG***************************/
int xp_door_debug(char *type,char *fun,char *param)
{
	int ret = 0;
	static uint16_t motor_resolution = 3000;
	static uint32_t profile_acceleration = 10000;
	static uint32_t profile_deceleration = 10000;

	if(strcmp(type,"door")!=0)return 0;
	if(strcmp(fun,"motor_resolution")==0){
		motor_resolution = strtoul(param, NULL, 10);
		return 1;
	}
	else if(strcmp(fun,"profile_acceleration")==0){
		profile_acceleration = strtoul(param, NULL, 10);
		return 1;
	}
	else if(strcmp(fun,"profile_deceleration")==0){
		profile_deceleration = strtoul(param, NULL, 10);
		return 1;
	}
	else if(strcmp(fun,"profile_velocity")==0){
		door1_speed = strtoul(param, NULL, 10);
		door2_speed = door1_speed;
		return 1;
  	}
	else if(strcmp(fun,"door_pan")==0){
		if(strcmp(param,"open")==0){
			ret = profile_velocity_config(&door_node_id_list[0], motor_resolution, profile_acceleration, 
										   profile_deceleration);
			if(0 != ret){
				return -1;
			}
			ret = profile_velocity_config(&door_node_id_list[1], motor_resolution, profile_acceleration,
											profile_deceleration);
			if(0 != ret){
				return -1;
			}
			ret = xp_door_open_common(&door_node_id_list[0], &door_node_id_list[1], DOOR_LEFT_OPEN_LIMIT2 , 
																DOOR_RIGHT_OPEN_LIMIT2, 20000, door2_speed);
			if(0 != ret){
				return -1;
			}
		}
		else if(strcmp(param,"close")==0){
			
			ret = profile_velocity_config(&door_node_id_list[0], motor_resolution, profile_acceleration, 
										   profile_deceleration);
			if(0 != ret){
				return -1;
			}
			ret = profile_velocity_config(&door_node_id_list[1], motor_resolution, profile_acceleration,
											profile_deceleration);
			if(0 != ret){
				return -1;
			}
			
			ret = xp_door_close_common(&door_node_id_list[0], &door_node_id_list[1], DOOR_LEFT_CLOSE_LIMIT2 , 
																	DOOR_RIGHT_CLOSE_LIMIT2, 20000, door2_speed);
			if(0 != ret){
				return -1;
			}
		}
        return 1;
  }
	else if(strcmp(fun,"door_rotate")==0){
		if(strcmp(param,"open")==0){
			ret = profile_velocity_config(&door_node_id_list[2], motor_resolution, profile_acceleration, 
										   profile_deceleration);
			if(0 != ret){
				return -1;
			}
			ret = profile_velocity_config(&door_node_id_list[3], motor_resolution, profile_acceleration,
											profile_deceleration);
			if(0 != ret){
				return -1;
			}
			xp_io_output_set(1, MAGNETIC_DOOR, 1);
			aos_msleep(3000);
			ret = xp_door_open_common(&door_node_id_list[2], &door_node_id_list[3], DOOR_LEFT_OPEN_LIMIT1 , 
																DOOR_RIGHT_OPEN_LIMIT1, 50000, door1_speed);
			if(0 != ret){
				return -1;
			}
		}
		else if(strcmp(param,"close")==0){
			
			ret = profile_velocity_config(&door_node_id_list[2], motor_resolution, profile_acceleration, 
										   profile_deceleration);
			if(0 != ret){
				return -1;
			}
			ret = profile_velocity_config(&door_node_id_list[3], motor_resolution, profile_acceleration,
											profile_deceleration);
			if(0 != ret){
				return -1;
			}
			
			xp_io_output_set(1, MAGNETIC_DOOR, 0);
			ret = xp_door_close_common(&door_node_id_list[2], &door_node_id_list[3], DOOR_LEFT_CLOSE_LIMIT1 , 
																	DOOR_RIGHT_CLOSE_LIMIT1, 50000, door1_speed);
			if(0 != ret){
				return -1;
			}
			
		}
        return 1;
  	}
	else if(strcmp(fun,"open")==0){
		xp_door_open();
	}
	else if(strcmp(fun,"close")==0){
		xp_door_close();
	}
	else if(strcmp(fun,"press_test")==0){
		if(strcmp(param,"start")==0){
			aos_sem_signal(&press_sem);
		}
		else if(strcmp(param,"stop")==0){
			press_stop=1;
		}
	}
		
	else{
			return 0;
	}
}


